/*
 * @Author: LVGRAPE
 * @LastEditors: LVGRAPE
 */

#include <stdint.h>
#include "arm_math.h"
#include "arm_const_structs.h"
#include <math.h>
#include <stdio.h>
#include <stdbool.h>
#include "dsp_test.h"
float ware1[SAMPLE_COUNT * 2];
float ware2[SAMPLE_COUNT * 2];
float ware3[SAMPLE_COUNT * 2];
float ware4[SAMPLE_COUNT * 2];

#define FFT_SIZE SAMPLE_COUNT // FFT大小

float32_t fftData[FFT_SIZE * 2];   // 输入数据
float32_t rfftData[FFT_SIZE * 2];  // 输入数据
float32_t output[FFT_SIZE * 2];    // 输出频谱
float32_t input[FFT_SIZE * 2];     // 输出频谱
float32_t magnitude[FFT_SIZE * 2]; // 幅度谱
float hanningWindow[FFT_SIZE * 2]; // 窗函数
float windowResult[FFT_SIZE * 2];
void stage_rfft_f32(arm_rfft_fast_instance_f32 *S, float32_t *p, float32_t *pOut);
void arm_cfft_radix8by2_f32(arm_cfft_instance_f32 *S, float32_t *p1);
void arm_cfft_radix8by4_f32(arm_cfft_instance_f32 *S, float32_t *p1);
void arm_radix8_butterfly_f32(float32_t *pSrc, uint16_t fftLen, const float32_t *pCoef, uint16_t twidCoefModifier);
void arm_bitreversal_32(uint32_t *pSrc, const uint16_t bitRevLen, const uint16_t *pBitRevTable);
// freq/index=dt_freq
int cal(int size, int freq)
{
    for (int i = 0; i < SAMPLE_COUNT; i++)
    {
        ware1[i] = ware2[i] = ware3[i] = ware4[i] = 0;
    }

    for (int i = 0; i < size; i++)
    {

        hanningWindow[i] = (0.5f - 0.5f * cosf(2 * M_PI * i / (size - 1)));
    }

    // 填充输入数据（示例：正弦波）
    float T = 1 / freq / size;
    for (int i = 0; i < size; i++)
    {
        // 0~0.004 1/250
        ware1[i] = sinf(2 * M_PI * i / 5) + sinf(2 * M_PI * i / 10) + sinf(2 * M_PI * i / 20);
        // input[i*i] = ware1[i];
        // input[2 * i + 1] = 0;
        // fftData[i] = ware1[i];
        // ware2[i] = ware1[i];
        // ware3[i] = ware1[i];
        // ware4[i] = ware1[i];

        // const uint8_t ringBufIdx = size - i;
        // arm_mult_f32(&ware1[i], &hanningWindow[0], &fftData[0], ringBufIdx);
        // if (i > 0)
        // {
        //     arm_mult_f32(&ware1[0], &hanningWindow[ringBufIdx], &fftData[ringBufIdx], i);
        // }
    }
    // arm_hanning_f32(hanningWindow,size);
    arm_mult_f32(ware1, hanningWindow, windowResult, size);
    for (int i = 0; i < size; i++)
    {
        input[i * 2] = windowResult[i];
        input[i * 2 + 1] = 0;
    }
    // 执行FFT计算
    arm_cfft_instance_f32 fft_instance;
    arm_cfft_init_f32(&fft_instance, size);
    arm_cfft_f32(&fft_instance, input, 0, 1);
    arm_cmplx_mag_f32(input, output, size);
    arm_scale_f32(output, 2, output, size);

    printf("index, ware1, input, output\n");
    for (int i = 0; i < size / 2; i++)
    {
        printf("%d, %f, %f, %f\n", i, ware1[i], input[i], output[i]);
    }
    // 初始化FFT实例
    // arm_rfft_fast_instance_f32 fftInstance;
    // arm_cfft_instance_f32 *Sint = &(fftInstance.Sint);

    // arm_rfft_fast_init_f32(&fftInstance, size);
    // // arm_radix8_butterfly_f32(fftData, size, Sint->pTwiddle, 1);
    // arm_rfft_fast_f32(&fftInstance, fftData, rfftData,0);
    // arm_cmplx_mag_f32(rfftData, fftData, size/2);

    // arm_bitreversal_32((uint32_t *)fftData, Sint->bitRevLength, Sint->pBitRevTable);
    // stage_rfft_f32(&fftInstance, fftData, rfftData);
    // arm_cmplx_mag_f32(rfftData, fftData, size / 2);

    // float fftResolution = (float)1 / size;

    // float fftSum = 0;
    // float fftWeightedSum = 0;
    // bool fftIncreasing = false;
    // uint8_t fftBinOffset = 0; //90 / fftResolution - 1.5f;
    // // iterate over fft data and calculate weighted indices
    // for (int i = 1 + fftBinOffset; i < size; i++) {
    //     const float data = fftData[i];
    //     const float prevData =fftData[i - 1];

    //     if (fftIncreasing || data > prevData *size) {
    //         float cubedData = data * data * data;

    //         // add previous bin before first rise
    //         if (!fftIncreasing) {
    //             cubedData += prevData * prevData * prevData;

    //             fftIncreasing = true;
    //         }

    //         fftSum += cubedData;
    //         // calculate weighted index starting at 1, not 0
    //         fftWeightedSum += cubedData * (i + 1);
    //     }
    // }

    // get weighted center of relevant frequency range (this way we have a better resolution than 31.25Hz)
    // if no peak, go to highest point to minimise delay
    // float centerFreq = 0;
    // float fftMeanIndex = 0;

    // if (fftSum > 0) {
    //     // idx was shifted by 1 to start at 1, not 0
    //     fftMeanIndex = (fftWeightedSum / fftSum) - 1;
    //     // the index points at the center frequency of each bin so index 0 is actually 16.125Hz
    //     centerFreq =fftMeanIndex * fftResolution;
    // }
    // arm_cmplx_conj_f32(input, output, size);
    // // 计算幅度谱
    // for (int i = 0; i < size / 2; i++) {
    //     magnitude[i] = sqrt(output[2 * i] * output[2 * i] + output[2 * i + 1] * output[2 * i + 1]);
    // }

    // 输出频谱
    // float max = 0;
    // int maxIndex = 0;
    // for (int i = 0; i < size; i++)
    // {
    // printf("fftData[%d]: %f\n", i, fftData[i]);
    // if (fftData[i] > max)
    // {
    //     max = fftData[i];
    //     maxIndex = i;
    // }
    //     printf("index:%d ware1:%f fftdata:%f rfftdata:%f \n", i, ware1[i], fftData[i], rfftData[i]);
    // }
    // printf("size:%d freq:%d index:%d max:%f freq:%d\n", size, freq, maxIndex, max, centerFreq);

    return 0;
}
/**
 * @brief 1024
 * 100Hz - 52  505.846771
 * 200Hz - 104 486.5168
 * 300Hz - 156 455.44256
 * 400Hz - 208 414.316315
 * 500Hz - 260 364.985077
 *
 *
 */
void ware_generate(void)
{
    // memset(ware1, 0, sizeof(float) * SAMPLE_COUNT);
    // memset(ware2, 0, sizeof(float) * SAMPLE_COUNT);
    // memset(ware3, 0, sizeof(float) * SAMPLE_COUNT);
    // memset(ware4, 0, sizeof(float) * SAMPLE_COUNT);

    // for (int size = 64; size <= 64; size *= 4)
    // {

    // for (int freq = 250; freq < 300; freq += 10)
    // {
    //     printf("cal freq: %d->", freq);
    //     ;
    // }
    cal(1024, 250);
    // }
    // cal(64, 300);

    for (int i = 0; i < SAMPLE_COUNT; i++)
    {

        ware2[i] = input[i];
        ware3[i] = output[i];
        ware4[i] = 0;
    }
    // memcpy(ware1, input, sizeof(float32_t) * SAMPLE_COUNT);
    // memcpy(ware2, input, sizeof(float32_t) * SAMPLE_COUNT);
    // memcpy(ware3, output, sizeof(float32_t) * SAMPLE_COUNT);
    // memcpy(ware4, magnitude, sizeof(float32_t) * SAMPLE_COUNT / 2);
}
